from socket import *
from matplotlib import pyplot as plt
import sys, os, struct
import math
from threading import Thread, Lock
from time import sleep
import numpy as np
from matplotlib.widgets import Button
import sklearn
import scipy.signal as signal
from sklearn.externals import joblib


sys.path.append(os.path.join(sys.path[0], '../..'))

from csi_data import CSI_Data
from csi_config import read_setting

PORT = int(read_setting('port'))

BUF_SIZE = 2048
client = socket(AF_INET, SOCK_STREAM, 0)
client.connect(('127.0.0.1', PORT))
totalcnt = 0


def getmod(arr):
    return np.abs(arr)

def getmodl(list):
    return [getmod(arr) for arr in list]

def getphase(arr):
    return np.angle(arr)

def getphasel(list):
    return [getphase(arr) for arr in list]

'''
计算相位差
'''
def get_phase_dif(csi1,csi2):
    csi_t = [0] * 30
    for i in range(30):
        if csi1[i] >= 0 and csi2[i] >= 0:
            if csi1[i] >= csi2[i]:
                temp = csi1[i] - csi2[i]
            else:
                temp = csi2[i] - csi1[i]
        elif csi1[i] > 0 and csi2[i] < 0:
            t = csi1[i] - np.pi
            if csi2[i] > t:
                temp = np.pi - csi2[i] + t
            else:
                temp = np.pi * 2 + csi2[i] - csi1[i]
        elif csi1[i] < 0 and csi2[i] > 0:
            t = csi2[i] - np.pi
            if csi1[i] >= t:
                temp = np.pi - csi1[i] + t
            else:
                temp = np.pi * 2 + csi1[i] - csi2[i]
        else:
            if csi1[i] >= csi2[i]:
                temp = csi1[i] - csi2[i]
            else:
                temp = csi2[i] - csi1[i]

        csi_t[i] = temp

    return csi_t

'''
相位扩展
'''
def phase_expend(dat):
    csi_pha_exp = [0] * 30
    csi_pha_exp[0] = dat[0]
    for i in range(1,30):
        if (dat[i] - dat[i-1])>=np.pi:
            csi_pha_exp[i] = csi_pha_exp[i-1] + (dat[i] - dat[i-1] - 2*np.pi)
        elif (dat[i] - dat[i-1])<= -np.pi:
            csi_pha_exp[i] = csi_pha_exp[i-1] + (dat[i] - dat[i-1] + 2 * np.pi)
        else:
            csi_pha_exp[i] = csi_pha_exp[i-1] + (dat[i] - dat[i-1])
    return csi_pha_exp


'''
相位线性变换
'''
def phase_linear_trans(dat):
    csi_pha_expend = phase_expend(dat)
    csi_pha_trans = [0] * 30
    m = [-28, -26, -24, -22, -20, -18, -16, -14, -12, -10,  -8,  -6,  -4, -2, -1, 1,  3,  5,  7,  9, 11, 13, 15, 17, 19, 21, 23, 25, 27, 28]

    k = (csi_pha_expend[29] - csi_pha_expend[0])/(m[29] - m[0]) #斜率
    b = np.sum(csi_pha_expend)/30 #截距

    for i in range(30):
        csi_pha_trans[i] = csi_pha_expend[i] - k * m[i] - b

    return csi_pha_trans

'''
Hampel滤波
'''
def hampel_filter(dat):
    frame = 5  # hample滤波器的窗口至少为7
    L = 3
    z_ham = []
    for i in range(frame, len(dat[frame:-frame])):
        l = dat[i - frame:i + frame]
        d = np.median(l)
        s = l - d
        m = np.median(s)
        MAD = 1.4826 * m
        if abs(l[0] - d) < L * MAD:
            z_ham.append(l[0])
        else:
            z_ham.append(d)
    s = np.arange(len(z_ham))
    b, a = signal.butter(3, 0.03, 'low')
    sf = signal.filtfilt(b, a, z_ham)
    # plt.plot(s, sf)
    z_ham = sf
    return z_ham

'''
时域转换到频域,获取频域特征--频域方差
'''
def freq_trans(dat):
    frame = 7
    z_fft = []
    for i in range(frame, len(dat[frame:-frame])):
        l = dat[i - frame:i + frame]
        l_fft = np.fft.fft(l)
        a= np.std(l_fft) #频域方差
        z_fft.append(a)

    return z_fft

'''
获取时域特征
'''
def time_trans(dat):
    frame = 7
    z_var = []
    for i in range(frame, len(dat[frame:-frame])):
        l = dat[i - frame:i + frame]
        l_var = np.std(l)
        z_var.append(l_var)

    return z_var

'''
计算信号短时能量
'''
def calEnergy(dat):
    frame = 7
    z_energy = []
    for i in range(frame, len(dat[frame:-frame])):
        l = dat[i - frame:i + frame]
        a = np.sum(np.square(l))
        z_energy.append(a)

    return z_energy



def read_t():
    alive = True
    while alive:
        try:
            buf = client.recv(BUF_SIZE)
            # print(buf)
            if buf is None:
                client.close()
                sys.exit(-2)
            i = struct.unpack('>H', buf[:2])
            print(len(buf))
            if i[0] == len(buf) - 2:  # correct package check - 1
                global totalcnt
                totalcnt += 1

                if buf[2] == 187:  # correct package check - 2
                    dat = CSI_Data(buf[3:])
                    csi_process_phase(dat)#观察相位差

        except struct.error as e:
            alive=False
            print(e)
            client.close()




# csi数据处理--两个天线30个子载波的相位
def csi_process_phase(dat):
    if dat is not None:
        print(dat.correct)
        csi_y.reverse()
        csi_classify.reverse()
        csi_t = phase_linear_trans(getphasel(dat.csi[0][0]))
        csi_m = get_model([csi_t])
        print(csi_m)
        csi_y.append(csi_t[10])
        csi_classify.append(csi_m[0])
        csi_y.remove(csi_y[0])
        csi_classify.remove(csi_classify[0])
        csi_y.reverse()
        csi_classify.reverse()
        line[0].set_ydata(csi_y)
        l[1][0].set_ydata(csi_classify)
        plt.draw()

'''
获取分类模型
'''
def get_model(csi_pha_train):
    global model
    predict = model.predict(csi_pha_train)
    return predict


plt.figure(1)
plt.ylim(-1 * 5, 5)
# plt.ylim(0, 100)
num = 200

l = [None, None, None]
# l[0] = plt.plot(range(1, num+1), [0 for _ in range(num)], 'ro')
l[1] = plt.plot(range(1, num+1), [0 for _ in range(num)], 'b-')
# plt.figure()
# plt.ylim(0,20)
# l[2] = plt.plot(range(1, num+1), [0 for _ in range(num)], 'g')

model = joblib.load("SVM_Model.m")


csi_y = [0] * num
csi_classify = [0] * num
line = plt.plot(range(0, num), [0 for _ in range(num)], 'r') #经数据处理之后的图线

read_thread = Thread(target=read_t)

read_thread.start()
plt.show()
read_thread.join()
